"""
img=cv2.imread('图片路径')
img=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
(threshold,BinaryImage) = cv2.threshold(img,0,255,cv2.THRESH_OTSU)
startpiont = (20,20)


CropImage = crop_picture(BinaryImage,1000)  
cv2.imshow("CropImage",CropImage)
cv2.imshow("img",img)
cv2.waitKey(0)
coordinates = GetPointList(CropImage,30,300,startpiont)#添加起点
print(coordinates)
path = AntsAlg(coordinates,50,200,0.3,1.2,0.5)

for i in range(0,len(coordinates)):
    cv2.line(CropImage,tuple(coordinates[int(path[i])]),
             tuple(coordinates[int(path[(i+1)%len(coordinates)])]),20,1)
    cv2.circle(CropImage,tuple(coordinates[int(path[i])]),7,128,-1)#绘制中心点
    cv2.putText(CropImage,str(int(path[i])),tuple(coordinates[int(path[i])]),cv2.FONT_HERSHEY_SIMPLEX, 0.75, 200, 2)
print(path)


cv2.imshow("CropImage",CropImage)
cv2.waitKey(0)

"""

import cv2
import numpy as np

def Distant(x,y,maxx,maxy):
    dst= np.array([[0,0],[0,maxy],[maxx,maxy],[maxx,0]],dtype=np.float32)
    res = -1.0
    mindist= float('inf') 
    for i in range(4):
        tmp  = np.sqrt((x-dst[i][0])**2+(y-dst[i][1])**2)
        if tmp < mindist:
            mindist = tmp
            res = i
    return res

def SortPoint(box,maxx:int,maxy:int):
    res = [[1.0,1.0],[1.0,1.0],[1.0,1.0],[1.0,1.0]]
    for i in range(4):
        res[Distant(box[i][0],box[i][1],maxx,maxy)] = box[i]
    return np.array(res,dtype=np.float32)


def crop_picture(BinaryImage,jumpVal):
    """
    image: 二值化后的图片并裁去多余数据
    """    
    img, contours, hierarchy= cv2.findContours(BinaryImage,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
    lastval = cv2.arcLength(contours[0],True)
    for i in range(1,len(contours)):
        arc = cv2.arcLength(contours[i],True)
        if np.abs(arc - lastval)>jumpVal: #范围若错误  改这个
            break
        
    rect = cv2.minAreaRect(contours[i-1])
    box = cv2.boxPoints(rect)      
    src= SortPoint(box,len(BinaryImage[0]),len(BinaryImage))
    dst= np.array([[0,0],[0,len(BinaryImage)],[len(BinaryImage[0]),len(BinaryImage)],
                   [len(BinaryImage[0]),0]],dtype=np.float32)
    m  = cv2.getPerspectiveTransform(src,dst)
    BinaryImage=cv2.warpPerspective(BinaryImage,m,(len(BinaryImage[0]),len(BinaryImage))) 
    return BinaryImage

def GetPointList(BinaryImage,pointMinacr,pointMaxacr,startpoint:tuple,endpoint:tuple):
    """
    BinaryImage : 裁切后的图片
    pointMinacr : 点在这图中的最小周长
    pointMinacr : 点在这图中的最大周长
    """
    img, contours, hierarchy= cv2.findContours(BinaryImage,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
    res = [list(startpoint)]
    
    for i in range(1,len(contours)):
        arc = cv2.arcLength(contours[i],True)
        if arc > pointMaxacr or arc<pointMinacr:
            continue      
        M = cv2.moments((contours[i]))
        center_x = int(M['m10']/M['m00']) #计算重心
        center_y = int(M['m01']/M['m00'])
        res.append([center_x,center_y])  
    res.append([endpoint[0],endpoint[1]])
    return np.array(res)

def GetDistantMat(coordinates):
    n  = len(coordinates)
    res = np.zeros((n,n)) 
    for i in range(n):
        for j in range(i,n):
            res[i,j]= res[j,i]=np.linalg.norm(coordinates[i]-coordinates[j])
    return res;    
    
    
def RandomPutAnts(AntNums:int,citysize:int):
    res = np.zeros((AntNums,citysize)).astype(int)
    UnVisited:list[set] = []
    for i in range(AntNums):
        UnVisited.append(set(range(citysize)))
        res[i,0] = np.random.randint(0,citysize)
        UnVisited[i].remove(res[i,0])
    return res,UnVisited

def ChosePath(Ettable:np.ndarray,pheromonetable:np.ndarray,nowcity:int,unvisited:set):
    n = Ettable.shape[0]
    tmp = Ettable[nowcity]*pheromonetable[nowcity]    
    possible = []
    for citys in unvisited:
        if tmp[citys]<0:
            tmp[citys]=0.0
        possible.append(tmp[citys]) 
    possible = possible/np.sum(possible) 
    return np.random.choice(list(unvisited),p=possible)
    
# TODO::还有bug
def AntsAlg(coordinates,AntNums:int,IterTurns:int,pheromoneDecree:float,Ke:float,Kp:float):
    n = len(coordinates)
    dtstanttable = GetDistantMat(coordinates)
    pheromonetable = np.ones((n,n)) 
    Ettable = 1/(dtstanttable**Ke + np.diag([float('inf')]*n))
    
    lengthbest = []  # 最佳路径长度
    lengthbestLenth = float('inf')
    
    for i in range(IterTurns):
        Ansplace,unvisted = RandomPutAnts(AntNums,n)
        LenthAnst = np.zeros(AntNums)
        for j in range(1,n):
            for Ants in range(AntNums):
                now = Ansplace[Ants][j-1]
                next = ChosePath(Ettable,pheromonetable,now,unvisted[Ants])
                unvisted[Ants].remove(next)
                Ansplace[Ants][j]=next
                pheromonetable[now][next] += Kp*pheromoneDecree/100
                LenthAnst[Ants]+= dtstanttable[now][next]  
            pass 
        
        
        if lengthbestLenth> LenthAnst.min():
            lengthbest = Ansplace[LenthAnst.argmin()]
            lengthbestLenth = LenthAnst.min()
        pheromonetable -=pheromoneDecree/100
    return lengthbest





